package mybox.engine.linefollower;

import static java.lang.Math.*;


import java.awt.Polygon;


public class Carstate {
	
	public double hPosX;
	public double hPosY;
	public double fahrzeugWinkelTau;	
	public double vPosX;
	public double vPosY;
	public double deltaS;
	public double alpha;
	public double psi;
	
	
	public Carstate(){}
	
	public Carstate(double initHPosX,
					double initHPosY,
					double fahrzeugWinkelTau,	
					double initVPosX,
					double initVPosY,
					double initAlpha,
					double initDeltaS
					) {
		this.hPosX=initHPosX;
		this.hPosY=initHPosY;
		this.fahrzeugWinkelTau=fahrzeugWinkelTau;
		this.vPosX=initVPosX;
		this.vPosY=initVPosY;
		this.alpha=initAlpha;
		this.deltaS=initDeltaS;
		this.psi=calcPsi();
	}
	
public Carstate nextCarState(Carstate currentState) {
		
		Carstate nextState=new Carstate(); 
		nextState.hPosX=currentState.hPosX+deltaS*cos(currentState.fahrzeugWinkelTau);
		nextState.hPosY=currentState.hPosY+deltaS*sin(currentState.fahrzeugWinkelTau);
		nextState.fahrzeugWinkelTau=currentState.fahrzeugWinkelTau+currentState.psi;
		nextState.vPosX=nextState.hPosX+Carline.carLengtha*cos(nextState.fahrzeugWinkelTau);
		nextState.vPosY=nextState.hPosY+Carline.carLengtha*sin(nextState.fahrzeugWinkelTau);
		nextState.alpha=currentState.alpha;
		nextState.deltaS=currentState.deltaS;
		nextState.psi=currentState.psi;
		return nextState;
	}

public Polygon getPolygon(int intFactor) {
	Polygon retPoly=new Polygon();
	retPoly.addPoint((int)round(hPosX*intFactor),(int)round(hPosY*intFactor));
	retPoly.addPoint((int)round(vPosX*intFactor),(int)round(vPosY*intFactor));
    return retPoly;
}

public double calcGamma() {
	return asin((Carline.carLengtha-deltaS)*sin(alpha)/Carline.carLengtha);
}

public double calcPsi() {
	return alpha-calcGamma(); 
}

}
